#include "scara.h"

scara::scara( int whichOne ) : sensors( whichOne )
{
 
}

scara::~scara()
{

}


int scara::scaraGoTo(float X, float Y, int orientation)
{

  char indentficador[2]; // identificador para o arduino saber de qual servo se trata
  float theta1,theta2,theta_ef,d;    

    timeOut = 10000000; // 10seg
  
    theta1 = atan2(Y,X)*(180.0/PI) + acos( (pow(X,2) + pow(Y,2) + pow(L1,2) - pow(L2,2) ) / (2*L1*sqrt(pow(X,2)+pow(Y,2))) )*(180.0/PI);

    theta2 = -180.0 + acos( (pow(L1,2) + pow(L2,2) - pow(X,2) - pow(Y,2) )/(2*L1*L2)  )*(180.0/PI) ;

    theta_ef = orientation - ( (int)theta1 + (int)theta2 );

    //d = Z - L0;

    cout << "theta1: " << (int)theta1 << " theta2: " << (int)theta2 << " theta_ef: " << (int)theta_ef << endl;

    if( isnan(theta1) || isnan(theta2) || isnan(theta_ef) || (theta1 < -27 || theta1 > 105) || (theta2 > 0 || theta2 < -130) )
    {
      cout << "Os valores nao pertencem ao espaco de trabalho do Scara!! Entre outros valores" << endl;
      return -1;
    }
    else
    {
      
      theta1 = (int)( (theta1+27.0)*(180.0/130.0) ); // "zeroServo = -27", servo gira 130graus, nao 180
      theta2 = (int)( 180 - (-theta2)*(180.0/130.0) );
      theta_ef = (int)( 90 + theta_ef ) ;
      //d = (int)d;
      
      theta1 = (theta1>180) ? 180 : theta1;
      theta1 = (theta1<0) ? 0 : theta1;
      theta2 = (theta2>180) ? 180 : theta2;
      theta2 = (theta2<0) ? 0 : theta2;
      theta_ef = (theta_ef>180) ? 180 : theta_ef;
      theta_ef = (theta_ef<0) ? 0 : theta_ef;

      cout << "junta01: " << theta1 << " junta02: " << theta2 << " efetuador: " << theta_ef << endl;
    }


    if( writeScaraServos(theta1,theta2,theta_ef) < 0 )
      return -1;

    
return 0;
}

int scara::pegaBloco()
{
  timeOut = 30000000; // 30seg
  if( writeCmd(0,SCARA_PEGA_BLOCO,ARDUINO_SCARA) < 0 )
    return -1;

return 0;  
}

int scara::deixaBloco()
{
  timeOut = 30000000; // 30seg
  if( writeCmd(0,SCARA_DEIXA_BLOCO,ARDUINO_SCARA) < 0 )
    return -1;
  
return 0;  
}

int scara::sobeEfetuador(int time)
{
  timeOut = 20000000; // 20seg
  if( writeCmd(time,SCARA_SOBE_EF,ARDUINO_SCARA) < 0 )
    return -1;
  
return 0;  
}

int scara::desceEfetuador(int time)
{
  timeOut = 20000000; // 20seg
  if( writeCmd(time,SCARA_DESCE_EF,ARDUINO_SCARA) < 0 )
    return -1;
  
return 0;  
}

struct timeval scara::le_t_envio()
{
  return t_envio;
}


bool scara::le_msgEnviada()
{
  return msgEnviada;
}

void scara::set_timeOutEstourado(bool arg)
{
  timeOutEstourado = arg;
}


int scara::le_serial()
{
  char res[20];
  
  if( msgEnviada==false )
  {
    if( serialport_read_until(fdScara,res,'!') == -1 )
      return -1;
    else
    {
      if( res[0]=='R' && res[1]=='E' && res[2]=='B' && res[3]=='O' && res[4]=='O' && res[5]=='T' )
	return 1;
      else
	return -1;
    }
  }

  if( arduinoResetou==true )
  {
    arduinoResetou = false;
    return 1;
  }
  else
    return -1;

  
}


long int scara::le_timeOut()
{
  return timeOut;
}

int scara::esperaStart()
{
  char res[10];
  int n=0;
  
  while( res[0]!='[' || res[1]!='B' || res[2]!='E' || res[3]!='G' || res[4]!='I' || res[5]!='N'|| res[6]!=']' )
    n = serialport_read_until(fdScara,res,'\n');

return n;
}

int scara::flush()
{
  int n;
  n = serialFlush(fdScara);
  
return n;  
}

